#include "process.h"

extern RosConnector *connector;

TrajPlanning traj_planning;

//QMap<int, QVector<std::pair<int, int>>> {
//    {1, {{1, 2}, {1, 3}}},
//    {2, {{2, 1}, {2, 2}}},
//    {3, {{2, 3}, {2, 4}}},
//    {4, {{3, 1}, {3, 2}}},
//    {5, {{3, 3}, {3, 4}}},
//    {6, {{5, 3}, {5, 4}, {4, 3}, {4, 4}}},
//    {7, {{5, 1}, {5, 2}, {4, 1}, {4, 2}}},
//    {8, {{6, 3}, {6, 4}}},
//    {9, {{6, 1}, {6, 2}}},
//    {10, {{1, 1}, {1, 4}}},
//};


STEP& operator+=(STEP& lhs, int i)
{
    lhs = STEP(int(lhs) + i);
    return lhs;
}

bool actuatorControl(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return true;
}
bool screwPosControl(QEventLoop* loop)
{
    vector<vector<vector<vector<double>>>> cartesian_position;
    vector<vector<vector<vector<double>>>> cartesian_orientation;
    cartesian_position.resize(10);
    for (int i=0;i<10;i++){
        cartesian_position[i].resize(4);
        for (int j=0;j<4;j++){
            cartesian_position[i][j].resize(8);
            for (int k=0;k<8;k++){
                cartesian_position[i][j][k].resize(3);
            }
        }
    }
    cartesian_orientation.resize(10);
    for (int i=0;i<10;i++){
        cartesian_orientation[i].resize(4);
        for (int j=0;j<4;j++){
            cartesian_orientation[i][j].resize(8);
            for (int k=0;k<8;k++){
                cartesian_orientation[i][j][k].resize(3);
            }
        }
    }
    cartesian_position={{{{601.13,529.75,0},{606.806,524.796,0},{612.482,519.842,0},{618.158,514.888,0},{623.834,509.934,0},{629.51,504.98,32.12}},{{629.51,504.98,32.12},{629.51,504.98, -27.88}}},
                        {{{536.65,575.08,0},{538.65,584.98,1.66}},{{538.65,584.98 ,1.66},{485.65, 584.98, 1.66}}},
                        {{{536.65,575.08,0},{533.65,584.98,-0.34}},{{533.65,584.98,-0.34},{454.15, 584.98, -157.84}}},
                        {{{594.66,571.27,0},{650.92,576.81,-10.29}},{{650.92,576.81,-10.29},{579.12,576.81,17 }},{{579.12,576.81,17},{514.15,576.81,17}},{{514.15,576.81,17},{472.4,576.81,174.5}}},
                        {{{601.13,529.75,0},{644.15,596.81,97}},{{644.15,596.81,97},{579.12, 596.81,97}},{{579.12,596.81,97},{514.15,596.81,97}},{{514.15,596.81,97},{454.15,596.81,97}}},
                        {{{601.13,529.75,0},{606.806,524.796,0},{612.482,519.842,0},{618.158,514.888,0},{623.834,509.934,0},{627.34,504.98,-32.12}},{{627.34,504.98,-32.12},{627.34,504.98,27.88}}},
                        {{{536.65,575.08,0},{537.2,584.98,-1.66}},{{537.2,584.98,-1.66},{484.2, 584.98, -1.66}}},
                        {{{536.65,575.08,0},{532.2,584.98,0.34}},{{532.2,584.98,0.34},{452.7, 584.98,157.84}}},
                        {{{594.66,571.27,0},{512.67,576.81,-17}},{{512.67,576.81,-17},{472.4,576.81,-174.5 }}},
                        {{{601.13,529.75,0},{512.67,596.81,-97}},{{512.67,596.81,-97},{452.7,596.81,-97 }}}};

    cartesian_orientation={{{{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{-0.35,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{-0.78,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0.698,0,-pi/2}}},
                        {{{0,0,-pi/2},{0.3,0,-pi/2}},{{0.3,0,-pi/2},{0.3,0,-pi/2}},{{0.3,0,-pi/2},{0.872,0,-pi/2}},{{0.872,0,-pi/2},{1,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0.35,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{0.78,0,-pi/2}}},
                        {{{0,0,-pi/2},{0,0,-pi/2}},{{0,0,-pi/2},{-0.698,0,-pi/2}}},
                        {{{0,0,-pi/2},{-0.872,0,-pi/2}},{{-0.872,0,-pi/2},{-0.95,0,-pi/2}}}};

    connector->kine.Param_joints_update_simulation(connector->motors);
    std::vector<double> traj_start_position;
    traj_start_position.resize(9);
    connector->i_log = connector->i_operate + 1;
    if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & connector->i_screw != 1) | connector->i_log == 7 | connector->i_log == 8){
        traj_start_position  = connector->kine.kec_joints2End_addh(connector->i_log);
    }else{
        traj_start_position  = connector->kine.kec_joints2End();
    }
    
    if (connector->i_screw != 0){              //在每个操作位的非首螺钉，需要记录起始位置
        for(int i=0; i<3; i++){
            cartesian_position[connector->i_operate][connector->i_screw][0][i] = traj_start_position[i];
            cartesian_orientation[connector->i_operate][connector->i_screw][0][i] = traj_start_position[i+3];
        }
    }
    vector<vector<double>> target_cartesian = traj_planning.catesianSpacePlan(cartesian_position[connector->i_operate][connector->i_screw],cartesian_orientation[connector->i_operate][connector->i_screw]);

    for (int i=0; i<target_cartesian.size(); i++){
        connector->kine.Param_writeto_Inv_sol(target_cartesian[i]);
        std::cout<<"位置:  "<<target_cartesian[i][0]<<"  "<<target_cartesian[i][1]<<"  "<<target_cartesian[i][2]<<"  "<<std::endl;
        
        if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & target_cartesian[i][0] < 594.66) | connector->i_log == 7 | connector->i_log == 8){
            connector->motors=connector->kine.Inv_sol_addh(connector->i_log);
        }else{
            connector->motors=connector->kine.Inv_sol();
        }
        connector->pub_all_robot(connector->motors);
        connector->pub_total_position(connector->motors,connector->i_operate);
    }
    
    if(connector->i_screw < cartesian_position[connector->i_operate].size()-1){
        connector->i_screw = connector->i_screw + 1;
    }else{
        connector->i_screw = 0;
        connector->i_operate = connector->i_operate + 1;
    }

    loop->quit();
    return false;
}
bool needToLoose(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return true;
}
double visualServoing(QEventLoop* loop)
{
    ros::spinOnce();
    // /********************************************************************存图片*/
    // uint8_t image_uint8[786432]; //512*512*3 根据相机像素尺寸进行修改；
	// for (int i=0;i<connector->image_data.size();i++)//转数据格式
	// {
	// 	image_uint8[i]=connector->image_data[i];
	// }
    
    // ofstream outfile;
    // outfile.open("/home/lqx/img_data.txt",ios::out);
    // for(int i=0;i<connector->image_data.size();i++)
    // {
    //     outfile<<to_string(image_uint8[i])<<endl;
    // }
    // outfile.close();
    // /********************************************************************存图片*/

    /********************************************读图片*/
    std::cout<<"-----------------------FLAG 开始视觉伺服----------------------"<<endl;
    ifstream inputfile("/home/lqx/img_data.txt");
    if(!inputfile.good())
    {
        cerr<<"读入期望图片失败"<<endl;
        return 0;
    }
    string line;
    uint8_t img_input[786432];
    uint64 num_pix=0;
    while(getline(inputfile,line))
    {
        img_input[num_pix]=atof(line.c_str());
        num_pix++;
    }
    cv::Mat desImg(connector->image_height,connector->image_width,CV_8UC3,img_input);
    cv::flip(desImg,desImg,0);
    cv::imshow("desImg",desImg);
    /********************************************读图片*/

    // //仿真相机参数
    double camera_sim_angle=Pi/3;
    double resolution_x=512;
    double focallength_pix=(resolution_x/2)/tan(camera_sim_angle/2);
    double u0=256,v0=256;
    std::cout<<"-----------------------FLAG vslaw声明----------------------"<<endl;
    VS_photometric vslaw(desImg,focallength_pix,u0,v0);//desImg:期望图像 focalLengthPIX，u0,v0:相机参数

    std::cout<<"-----------------------FLAG 开始控制----------------------"<<endl;
    vslaw.VScontrol_simulation();
    loop->quit();
    return 0;
}
bool checkVisualServoing(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return false;
}
bool motionControl_Down(QEventLoop* loop)
{
    connector->kine.Param_joints_update_simulation(connector->motors);
    std::vector<double> down_pos_direc;
    down_pos_direc.resize(9);
    if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & connector->i_screw != 1) | connector->i_log == 7 | connector->i_log == 8){
        down_pos_direc  = connector->kine.kec_joints2End_addh(connector->i_log);
    }else{
        down_pos_direc  = connector->kine.kec_joints2End();
    }
    std::vector<double> down_pos;
    down_pos.resize(6);
    traj_planning.moveDirection_ver.resize(3);
    traj_planning.moveDirection_ver[0] = down_pos_direc[6];
    traj_planning.moveDirection_ver[1] = down_pos_direc[7];
    traj_planning.moveDirection_ver[2] = down_pos_direc[8];
    for (int i=0; i<6; i++){
        down_pos[i] = down_pos_direc[i];
    }
    double state_length = 0;
    int state_target = 0;
    traj_planning.state_hold = 0;
    while(traj_planning.state_hold < 5){
        state_length = state_length + 1;
        if(state_length > 160){       ////////////实际应改为 state_target = 微动开关参数
            state_target = 1;
        }
        down_pos = traj_planning.catesianSpacePlanOnLine(down_pos,state_target);
        connector->kine.Param_writeto_Inv_sol(down_pos);
        if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & connector->i_screw != 1) | connector->i_log == 7 | connector->i_log == 8){
            connector->motors=connector->kine.Inv_sol_addh(connector->i_log);
        }else{
            connector->motors=connector->kine.Inv_sol();
        }
        connector->pub_all_robot(connector->motors);
        connector->pub_total_position(connector->motors,connector->i_log); //仿真中控制关节速度
    }
    loop->quit();
    return true;
}
bool motionControl_Up(QEventLoop* loop)
{
    connector->kine.Param_joints_update_simulation(connector->motors);
    std::vector<double> up_pos_direc;
    up_pos_direc.resize(9);
    if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & connector->i_screw != 1) | connector->i_log == 7 | connector->i_log == 8){
        up_pos_direc  = connector->kine.kec_joints2End_addh(connector->i_log);
    }else{
        up_pos_direc  = connector->kine.kec_joints2End();
    }
    std::vector<double> up_pos;
    up_pos.resize(6);
    traj_planning.moveDirection_ver[0] = -traj_planning.moveDirection_ver[0];
    traj_planning.moveDirection_ver[1] = -traj_planning.moveDirection_ver[1];
    traj_planning.moveDirection_ver[2] = -traj_planning.moveDirection_ver[2];
    for (int i=0; i<6; i++){
        up_pos[i] = up_pos_direc[i];
    }
    double state_length = 0;
    int state_target = 0;
    traj_planning.state_hold = 0;
    while(traj_planning.state_hold < 5){
        state_length = state_length + 1;
        if(state_length > 160){     
            state_target = 1;
        }
        up_pos = traj_planning.catesianSpacePlanOnLine(up_pos,state_target);
        connector->kine.Param_writeto_Inv_sol(up_pos);
        if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & connector->i_screw != 1) | connector->i_log == 7 | connector->i_log == 8){
            connector->motors=connector->kine.Inv_sol_addh(connector->i_log);
        }else{
            connector->motors=connector->kine.Inv_sol();
        }
        connector->pub_all_robot(connector->motors);
        connector->pub_total_position(connector->motors,connector->i_log); //仿真中控制关节速度
    }
    loop->quit();
    return true;
}
bool motorControl(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return true;
}
bool checkMotorElectric(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return true;
}
bool looseScrew(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return true;
}
bool checkScrew(QEventLoop* loop)
{
    QThread::msleep(300);
    loop->quit();
    return true;
}
void trajectoryPlan(QEventLoop* loop)
{
    // //给定初始关节空间、笛卡尔空间 特征点
    // //需要读出每个电机的初始角，赋给q_array
    // vector<vector<double>> q_array={{ connector->motors[0],M_PI*3/6} , { connector->motors[1],M_PI*3/6 }, { connector->motors[2],M_PI*3/6 }, { connector->motors[3],M_PI*4/6 }, { connector->motors[4],M_PI*5/6 }, { connector->motors[5],M_PI },{ connector->motors[6],M_PI },{ connector->motors[7],M_PI },{ connector->motors[8],M_PI },{ connector->motors[9],M_PI }};
    // vector<double> time={0, 9};

    // vector<vector<double>> q_traj = traj_planning.jointSpacePlan(q_array, time);
    // for (int i=0; i<q_traj.size(); i++){
    //     for(int j=0;j<10;++j){
    //         connector->motors[j]=q_traj[i][j];
    //     }
    //     connector->pub_all_robot(connector->motors);
    //     connector->pub_total_position(connector->motors,connector->i_log); //仿真中控制关节速度
    // }

    // loop->quit();
}

bool manualModeExample(QEventLoop* loop)
{   
    // 添加手动位置初始
    connector->kine.Param_joints_update_simulation(connector->motors);
    if (connector->i_log == 2 | connector->i_log == 3 | (connector->i_log == 4 & connector->i_screw != 1) | connector->i_log == 7 | connector->i_log == 8){
        connector->set_manual_start(connector->kine.kec_joints2End_addh(connector->i_log));
    }else{
        connector->set_manual_start(connector->kine.kec_joints2End());
    }
    while(connector->manualPattern){
        ros::spinOnce();
    }
    loop->quit();
    return true;
}

inline void saveContext(context* ct, int opIdx, int screwIdx, STEP step){
    ct->curOpIdx = opIdx;
    ct->curScrewIdx = screwIdx;
    ct->curStep = step;
}

void mockPanelState(DIRECT& direct, Viewer* view){
    QVector<DIRECT> allDirects{
        UP,
        DOWN,
        FORWARD,
        BACK,
        LEFT,
        RIGHT,
        UPFORWARD,
        UPBACK,
        UPLEFT,
        UPRIGHT,
        DOWNFORWARD,
        DOWNBACK,
        DOWNLEFT,
        DOWNRIGHT,
        ORIGIN
    };
    QTimer* timer = new QTimer();
    QObject::connect(timer, &QTimer::timeout, view, [allDirects,&direct](){
        static int idx=0;
        direct = allDirects[idx%allDirects.size()];
        idx++;
    });
    timer->setInterval(5);
    timer->start();

    view->bindPanelState(direct);
}

Viewer* initViewer(QQmlApplicationEngine &engine){
    return new Viewer(engine);
}

void processInAutoMode(Viewer* view, context* curContext)
{
    // 开始流程
    QEventLoop loop;

    // 切换手\自动状态位
    view->switchPattern(false);

    // 上电状态位
    view->setStatusLight(POWERON);

    // 从当前上下文开始
    for(int opIdx = curContext->curOpIdx; opIdx <= 10; ++opIdx){
        // 操作位指示灯开始闪烁
        view->vecOpLeds[opIdx]->setStatus(BLINK);

        // 电推杆控制子函数
        if(curContext->curStep == S1){
            QFuture<bool> result = QtConcurrent::run(&actuatorControl, &loop);
            loop.exec();
            if(!result){
                saveContext(curContext, opIdx, 0, S1);
                return;
            }
        }

        // 从当前上下文螺钉id开始
        for(int screwIdx = curContext->curScrewIdx; screwIdx < op2Led[opIdx].size(); ++screwIdx){
            // // 弹窗
            // if(1){
            //     QString content = "切换到自动模式";
            //     view->infoBox->setText(content);
            //     int val = view->infoBox->exec();
            // }
            // 螺钉指示灯开始闪烁
            // std::cout << "start" << std::endl;
            view->vecScrewLeds[op2Led[opIdx][screwIdx].first][op2Led[opIdx][screwIdx].second]->setStatus(BLINK);

            // 状态复位
            {
                view->setStatusDark(PLACED);
                view->setStatusDark(START);
                view->setStatusDark(LOOSE);
                view->setStatusDark(STOP);
            }
            // std::cout<<"step flag"<<to_string(curContext->curStep)<<endl;

            switch(curContext->curStep){
                case S1 : {
                    // 弹窗
                    if(1){
                        // QString content = "n操作位--n螺钉";
                        QString content = QString("%1操作位--%2螺钉").arg((connector->i_operate+1)).arg((connector->i_screw+1));
                        view->infoBox->setText(content);
                        int val = view->infoBox->exec();
                    }
                }
                case S2 : {
                    // 第screwIdx个螺钉预位姿规划子函数
                    {
                        QFuture<bool> result = QtConcurrent::run(&screwPosControl, &loop);
                        // std::cout<<"预位姿轨迹规划结束"<<endl;
                        loop.exec();
                        if(!result){
                            ros::spinOnce();
                            saveContext(curContext, opIdx, screwIdx, S3);
                            connector->manualPattern=true;
                            return;
                        }
                    }

                    // 当前螺钉是否需要拧松
                    {
                        QFuture<bool> result = QtConcurrent::run(&needToLoose, &loop);
                        loop.exec();
                        if(!result){
                            continue;
                        }
                    }
                }
                case S3 : {
                    // 弹窗
                    if(1){
                        QString content = "进入视觉伺服";
                        view->infoBox->setText(content);
                        int val = view->infoBox->exec();
                    }

                    // 视觉伺服精确定位
                    {
                        QtConcurrent::run(&visualServoing, &loop);
                        loop.exec();
                    }
                    // 伺服定位是否成功
                    {
                        QFuture<bool> result = QtConcurrent::run(&checkVisualServoing, &loop);
                        loop.exec();
                        if(!result){
                            ros::spinOnce();
                            saveContext(curContext, opIdx, screwIdx, S4);
                            connector->manualPattern=true;
                            return;
                        }
                    }
                }
                case S4 : {
                    // 弹窗
                    if(1){
                        QString content = "定位结束";
                        view->infoBox->setText(content);
                        int val = view->infoBox->exec();
                    }

                    // 拧松电机微动  置于S4
                    {
                        QtConcurrent::run(&motorControl, &loop);
                        loop.exec();
                    }
                    std::cout<<"--------------套筒下落----------------"<<endl;
                    // 末端套筒往下运动直至到位
                    {
                        QFuture<bool> result = QtConcurrent::run(&motionControl_Down, &loop);
                        loop.exec();
                        if(!result){
                            saveContext(curContext, opIdx, screwIdx, S4);
                            return;
                        }
                    }

                    // ‘到位’状态变亮
                    view->setStatusLight(PLACED);
                }
                case S5 : {
                    // 拧松电机微动并判断套上
                    {
                        QFuture<bool> result = QtConcurrent::run(&motorControl, &loop);
                        loop.exec();
                        QFuture<bool> result2 = QtConcurrent::run(&checkMotorElectric, &loop);
                        loop.exec();
                        if(!result || !result2){
                            saveContext(curContext, opIdx, screwIdx, S5);
                            return;
                        }
                    }

                    // ‘到位’状态变亮
                    view->setStatusLight(PLACED);
                    // ‘启动’状态变亮
                    view->setStatusLight(START);
                }
                case S6 : {
                    // 螺钉拧松
                    {
                        QFuture<bool> result = QtConcurrent::run(&motorControl, &loop);
                        loop.exec();
                        QFuture<bool> result2 = QtConcurrent::run(&checkMotorElectric, &loop);
                        loop.exec();
                        if(!result || !result2){
                            saveContext(curContext, opIdx, screwIdx, S6);
                            return;
                        }
                    }

                    // ‘到位’状态变亮
                    view->setStatusLight(PLACED);
                    // ‘启动’状态变亮
                    view->setStatusLight(START);
                    // ‘拧松’状态变亮
                    view->setStatusLight(LOOSE);
                }
                case S7 : {
                    // 拧松电机停转
                    {
                        QFuture<bool> result = QtConcurrent::run(&motorControl, &loop);
                        loop.exec();
                        if(!result){
                            saveContext(curContext, opIdx, screwIdx, S7);
                            return;
                        }
                    }

                    // ‘到位’状态变亮
                    view->setStatusLight(PLACED);
                    // ‘启动’状态变亮
                    view->setStatusLight(START);
                    // ‘拧松’状态变亮
                    view->setStatusLight(LOOSE);
                    // ‘停止’状态变亮
                    view->setStatusLight(STOP);
                }
                case S8 : {
                    // 末端套筒往上运动直至到位
                    std::cout<<"--------------套筒上升----------------"<<endl;
                    {
                        QFuture<bool> result = QtConcurrent::run(&motionControl_Up, &loop);
                        loop.exec();
                        if(!result){
                            saveContext(curContext, opIdx, screwIdx, S8);
                            return;
                        }
                    }
                }
                case S9 : {
                    // 检查当前螺钉是否拧松
                    {
                        QFuture<bool> result = QtConcurrent::run(&checkScrew, &loop);
                        loop.exec();
                        if(!result){
                            saveContext(curContext, opIdx, screwIdx, S9);
                            return;
                        }
                    }
                }
            }
            // std::cout << "off" << std::endl;
            // 螺钉指示灯关闭
            
            view->vecScrewLeds[op2Led[opIdx][screwIdx].first][op2Led[opIdx][screwIdx].second]->setStatus(OFF);

            {
                saveContext(curContext, opIdx, screwIdx, S9);
            }

            {
                // 假设当前螺钉操作完毕, 更新界面指示灯，同时更新上下文令螺钉id加1
                //view->vecScrewLeds[op2Led[curContext->curOpIdx][curContext->curScrewIdx].first][op2Led[curContext->curOpIdx][curContext->curScrewIdx].second]->setStatus(OFF);
                curContext->curScrewIdx += 1;

                // 判断是否跳转到下一个操作位
                if(curContext->curScrewIdx == op2Led[curContext->curOpIdx].size()){
                    // 更新界面指示灯
                    view->vecOpLeds[curContext->curOpIdx]->setStatus(OFF);
                    curContext->curOpIdx += 1;

                    // 判断所有操作位是否都已遍历
                    if(curContext->curOpIdx == 11){
                        curContext->curOpIdx = 10;
                        curContext->curScrewIdx = op2Led[curContext->curOpIdx].size() - 1;
                        curContext->curStep = FIN;
                        return;
                    }
                    // 从当前操作位第一颗螺钉开始
                    curContext->curScrewIdx = 0;
                    curContext->curStep = S1;
                }else{
                    curContext->curStep = S1;
                }
            }
        }
        // 操作位指示灯关闭
        view->vecOpLeds[opIdx]->setStatus(OFF);
    }

    // 机械臂回到初始位姿
    {
        QtConcurrent::run(&trajectoryPlan, &loop);
        loop.exec();
    }
    saveContext(curContext, 10, op2Led[10].size() - 1, FIN);
}

void processInManualMode(Viewer* view, context* curContext)
{
    QEventLoop loop;

    if(1){
        QString content = "请手动调整";
        view->infoBox->setText(content);
        int val = view->infoBox->exec();
    }

    // 切换手\自动状态位
    view->switchPattern(true);


    {
        QFuture<bool> result = QtConcurrent::run(&manualModeExample, &loop);
        loop.exec();

        if(!result){
            //
            
            return;
        }else{
            // curContext->curStep = STEP(int(curContext->curStep) + 1);
            // // 假设当前螺钉操作完毕, 更新界面指示灯，同时更新上下文令螺钉id加1
            // view->vecScrewLeds[op2Led[curContext->curOpIdx][curContext->curScrewIdx].first][op2Led[curContext->curOpIdx][curContext->curScrewIdx].second]->setStatus(OFF);
            // curContext->curScrewIdx += 1;

            // // 判断是否跳转到下一个操作位
            // if(curContext->curScrewIdx == op2Led[curContext->curOpIdx].size()){
            //     // 更新界面指示灯
            //     view->vecOpLeds[curContext->curOpIdx]->setStatus(OFF);
            //     curContext->curOpIdx += 1;

            //     // 判断所有操作位是否都已遍历
            //     if(curContext->curOpIdx == 11){
            //         curContext->curOpIdx = 10;
            //         curContext->curScrewIdx = op2Led[curContext->curOpIdx].size() - 1;
            //         curContext->curStep = FIN;
            //         return;
            //     }
            //     // 从当前操作位第一颗螺钉开始
            //     curContext->curScrewIdx = 0;
            //     curContext->curStep = S1;
            // }
        }
    }


}
